
#include <physics/physics.hpp>
#include <physics/user_data/user_data.hpp>

body_ptr create_body( world_ptr world, b2BodyDef const * const definition )
{
	b2Body * body = world->CreateBody( definition );

	world_weak_ptr weak_world = world;
	body_ptr out( body, [weak_world]( b2Body * body )
	{
		body->SetUserData( nullptr );

		world_ptr world = weak_world.lock( );
		if ( world != nullptr )
		{
			world->DestroyBody( body );
		}
		
		body = nullptr;
	} );

	return out;
}

joint_ptr create_joint( world_ptr world, b2JointDef const * const definition )
{
	b2Joint * joint = world->CreateJoint( definition );

	world_weak_ptr weak_world = world;
	joint_ptr out( joint, [weak_world]( b2Joint * joint )
	{
		joint->SetUserData( nullptr );

		world_ptr world = weak_world.lock( );
		if ( world != nullptr )
		{
			world->DestroyJoint( joint );
		}
		
		joint = nullptr;
	} );

	return out;
}

void contact_listener::BeginContact( b2Contact * contact )
{
	auto user_data_a = static_cast<base_user_data*>( contact->GetFixtureA( )->GetBody( )->GetUserData( ) );
	auto user_data_b = static_cast<base_user_data*>( contact->GetFixtureB( )->GetBody( )->GetUserData( ) );

	if ( user_data_a != nullptr )
	{
		user_data_a->on_contact( contact, user_data_a, user_data_b );
	}

	if ( user_data_b != nullptr )
	{
		user_data_b->on_contact( contact, user_data_b, user_data_a );
	}
}

#if PHYSICS_DEBUG

#include <window/window.hpp>

#pragma warning(push)
#pragma warning(disable:4244)
#pragma warning(disable:4996)

debug_draw::debug_draw( )
{
	SetFlags(	e_shapeBit
			|	e_jointBit
			|	e_pairBit
			|	e_centerOfMassBit );
}

void debug_draw::DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color)
{
	sf::Shape polygon;
	for ( int32 i = 0; i < vertexCount; ++i )
	{
		polygon.AddPoint(	sf::Vector2f( vertices[i].x * g_scale_b2_position, vertices[i].y * g_scale_b2_position ),
							sf::Color( ),
							sf::Color( color.r * 255, color.g * 255, color.b * 255, 128 ) );
	}
	polygon.EnableFill( false );
	polygon.EnableOutline( true );
	polygon.SetOutlineWidth( 2.0f );

	g_window->Draw( polygon );
}

void debug_draw::DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color)
{
	sf::Shape polygon;
	for ( int32 i = 0; i < vertexCount; ++i )
	{
		polygon.AddPoint(	sf::Vector2f( vertices[i].x * g_scale_b2_position, vertices[i].y * g_scale_b2_position ),
							sf::Color( color.r * 128, color.g * 128, color.b * 128, 64 ),
							sf::Color( color.r * 255, color.g * 255, color.b * 255, 128 ) );
	}
	polygon.EnableFill( true );
	polygon.EnableOutline( true );
	polygon.SetOutlineWidth( 2.0f );

	g_window->Draw( polygon );
}

void debug_draw::DrawCircle(const b2Vec2& center, float32 radius, const b2Color& color)
{
	sf::Shape shape( sf::Shape::Circle(	sf::Vector2f( center.x * g_scale_b2_position, center.y * g_scale_b2_position ),
										radius * g_scale_b2_position,
										sf::Color( color.r * 128, color.g * 128, color.b * 128, 64 ),
										2.0f,
										sf::Color( color.r * 255, color.g * 255, color.b * 255, 128 ) ) );

	shape.EnableFill( false );
	shape.EnableOutline( true );
	shape.SetOutlineWidth( 2.0f );

	g_window->Draw( shape );
}

void debug_draw::DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color)
{
	{
		sf::Shape shape( sf::Shape::Circle(	sf::Vector2f( center.x * g_scale_b2_position, center.y * g_scale_b2_position ),
											radius * g_scale_b2_position,
											sf::Color( color.r * 128, color.g * 128, color.b * 128, 64 ),
											2.0f,
											sf::Color( color.r * 255, color.g * 255, color.b * 255, 128 ) ) );

		shape.EnableFill( true );
		shape.EnableOutline( true );
		shape.SetOutlineWidth( 2.0f );

		g_window->Draw( shape );
	}

	{
		b2Vec2 const point = center + ( radius * axis );

		sf::Vector2f c( center.x * g_scale_b2_position, center.y * g_scale_b2_position );
		sf::Vector2f p( point.x * g_scale_b2_position, point.y * g_scale_b2_position );

		sf::Shape shape( sf::Shape::Line(	c,
											p,
											2.0f,
											sf::Color( color.r * 255, color.g * 255, color.b * 255, 128 ) ) );

		shape.EnableFill( true );
		shape.EnableOutline( false );
		shape.SetOutlineWidth( 2.0f );

		g_window->Draw( shape );
	}
}

void debug_draw::DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color)
{
	sf::Shape shape( sf::Shape::Line(	sf::Vector2f( p1.x * g_scale_b2_position, p1.y * g_scale_b2_position ),
										sf::Vector2f( p2.x * g_scale_b2_position, p2.y * g_scale_b2_position ),
										2.0f,
										sf::Color( color.r * 255, color.g * 255, color.b * 255 ) ) );

	shape.EnableFill( true );
	shape.EnableOutline( false );
	shape.SetOutlineWidth( 2.0f );

	g_window->Draw( shape );
}

void debug_draw::DrawTransform(const b2Transform& xf)
{
	const float32 k_axisScale = 1.0f;
	DrawSegment(	xf.position,
					xf.position + ( k_axisScale * xf.R.col1 ),
					b2Color(1.0f, 0.0f, 0.0f) );

	DrawSegment(	xf.position,
					xf.position + ( k_axisScale * xf.R.col2 ),
					b2Color(0.0f, 1.0f, 0.0f) );
}

void debug_draw::DrawPoint(const b2Vec2& p, float32 size, const b2Color& color)
{
	sf::Vector2f min_(	( g_scale_b2_position * p.x ) - ( size * 0.5f ),
						( g_scale_b2_position * p.y ) - ( size * 0.5f ) );

	sf::Vector2f max_(	( g_scale_b2_position * p.x ) + ( size * 0.5f ),
						( g_scale_b2_position * p.y ) + ( size * 0.5f ) );

	sf::Shape shape( sf::Shape::Rectangle( min_, max_, sf::Color( color.r * 255, color.g * 255, color.b * 255 ) ) );

	shape.EnableFill( true );
	shape.EnableOutline( false );
	shape.SetOutlineWidth( 2.0f );

	g_window->Draw( shape );
}

void debug_draw::DrawString(int x, int y, const char* string, ...)
{
	char buffer[128];

	va_list arg;
	va_start(arg, string);
	vsprintf_s(buffer, 128, string, arg);
	va_end(arg);
	
	sf::String text( static_cast<char *>(buffer) );
	text.SetPosition( static_cast<float>(x), static_cast<float>(y) );
	g_window->Draw( text );
}

void debug_draw::DrawAABB(b2AABB* aabb, const b2Color& color)
{
	sf::Vector2f min_(	g_scale_b2_position * aabb->lowerBound.x,
						g_scale_b2_position * aabb->lowerBound.y );

	sf::Vector2f max_(	g_scale_b2_position * aabb->upperBound.x,
						g_scale_b2_position * aabb->upperBound.y );

	sf::Shape shape( sf::Shape::Rectangle(	min_,
											max_,
											sf::Color( color.r * 255, color.g * 255, color.b * 255 ),
											2.0f,
											sf::Color( color.r * 255, color.g * 255, color.b * 255 ) ) );

	shape.EnableFill( false );
	shape.EnableOutline( true );
	shape.SetOutlineWidth( 2.0f );

	g_window->Draw( shape );
}

#pragma warning(pop)

#endif
